//
// Created by ze on 2023/11/8.
//
#include "UPRE_MOVEBASE.h"
#include "math.h"
#include "ultrasonic.h"

#include "cmsis_os.h"
#include "UPRE_LOCATION.h"
#include "UPRE_PATH.h"
#include "UPRE_PID.h"
#include "bsp_fbcf.h"
#include "UPRE_MOTOR.h"
#include "UPRE_Task_Flow.h"

void Output_dir(WheelSys WheelBase);

Steering2Zero_ Steering2Zero = {0};
//轮电机归正锁
static bool flagLockF = true;
static bool flagLockL = true;
static bool flagLockR = true;
bool TurnLock = true; //等舵电机转到与路径所需角度相同时轮电机解锁
bool SpeedLock = false;


/* 底盘参数初始化*/
/**********************************************************************************************************/
//三舵轮底盘
WheelSys SteeringBase = {
    .LeftFront = {.RotationRadius = 1.f, .PositiveDir = -90.f, .currentPos = 0, .targetVel = {0}},
    //1    PositiveDIr是安装位置(顺负逆正)
    //        .RightFront = {.RotationRadius = 1.f, .PositiveDir = 45.f, .currentPos = 0, .targetVel = {0}},//2
    .LeftRear = {.RotationRadius = 1.f, .PositiveDir = 65.5f, .currentPos = 0, .targetVel = {0}}, //3
    .RightRear = {.RotationRadius = 1.f, .PositiveDir = 114.5f, .currentPos = 0, .targetVel = {0}}, //2
};

//四全向轮底盘
static WheelSys OminiBase = {
    .LeftFront = {.RotationRadius = 1.f, .PositiveDir = 135.f, .currentPos = 0, .targetVel = {0}}, //1
    .LeftRear = {.RotationRadius = 1.f, .PositiveDir = -135.f, .currentPos = 0, .targetVel = {0}}, //2
    .RightRear = {.RotationRadius = 1.f, .PositiveDir = -45.f, .currentPos = 0, .targetVel = {0}}, //3
    .RightFront = {.RotationRadius = 1.f, .PositiveDir = 45.f, .currentPos = 0, .targetVel = {0}}, //4
};
//三全向轮底盘
static WheelSys OminiBase3 = {
    .LeftFront = {.RotationRadius = 1.f, .PositiveDir = 90.f, .currentPos = 0, .targetVel = {0}}, //1
    .RightRear = {.RotationRadius = 1.f, .PositiveDir = -30.f, .currentPos = 0, .targetVel = {0}}, //2
    .LeftRear = {.RotationRadius = 1.f, .PositiveDir = -150.f, .currentPos = 0, .targetVel = {0}}, //3
};
/**
 * @brief 轮子输出速度
 * @
*/
void Output_Speed(WheelSys WheelType) {
    static float Last_F_Vel, Last_L_Vel, Last_R_Vel = 0.f;
#if MODE == 0


    // Output_dir(WheelType);
    if (WheelType.LeftFront.targetVel.size - Last_F_Vel >= 65535.f)
        WheelType.LeftFront.targetVel.size = Last_F_Vel;
    if (WheelType.LeftRear.targetVel.size - Last_L_Vel >= 65535.f)
        WheelType.LeftRear.targetVel.size = Last_L_Vel;
    if (WheelType.RightRear.targetVel.size - Last_R_Vel >= 65535.f)
        WheelType.RightRear.targetVel.size = Last_R_Vel;
    if (TurnLock && ((SteeringBase.LeftFront.targetVel.dir * 0.875944f - (SteeringControl.Front.pos)) <= 5.f)
        && ((SteeringBase.LeftRear.targetVel.dir * 0.875944f - (SteeringControl.Left.pos)) <= 5.f)
        && ((SteeringBase.RightRear.targetVel.dir * 0.875944f - (SteeringControl.Right.pos)) <= 5.f)) {
        TurnLock = false;
    }
    if (SpeedLock && ((SLOWSTARTSPEED - fabs(SteeringControl.Front.spd) * 1000) <= 5.f)
        && ((SLOWSTARTSPEED - fabsf(SteeringControl.Left.spd) * 1000) <= 5.f)
        && ((SLOWSTARTSPEED - fabs(SteeringControl.Right.spd) * 1000) <= 5.f)) {
        SpeedLock = false;
    }
#if STEERING_LOCK == 1
    fbcf_arm(0x01);
    fbcf_arm(0x03);
    fbcf_arm(0x02);
#endif

#if LUN_MOTOR_LOCK_OFF == 0

    if (!TurnLock) //TODO
    {
#endif
        // printf("%f\t%f\t%f\t%f\t%f\r%f\r\n",WheelType.LeftRear.targetVel.size,WheelType.LeftFront.targetVel.size,WheelType.RightRear.targetVel.size,SteeringControl.Front.spd,SteeringControl.Left.spd,SteeringControl.Right.spd);
        if (!SpeedLock) {
            fbcf_set_pos_spd(0x01, WheelType.LeftFront.targetVel.dir * 0.875944f,
                             WheelType.LeftFront.targetVel.size * 0.001f);
            osDelay(3);
            // HAL_Delay(1);
            fbcf_set_pos_spd(0x03, WheelType.LeftRear.targetVel.dir * 0.875944f,
                             WheelType.LeftRear.targetVel.size * 0.001f);
            // fbcf_set_pos_spd(0x3,WheelType.LeftRear.targetVel.dir *0.875944f,-5.f);
            osDelay(3);
            // HAL_Delay(1);
            fbcf_set_pos_spd(0x02, WheelType.RightRear.targetVel.dir * 0.875944f,
                             WheelType.RightRear.targetVel.size * 0.001f);
            // fbcf_set_pos_spd(0x2,WheelType.RightRear.targetVel.dir*0.875944f,-5);
            osDelay(3);
        } else {
            if(WheelType.LeftFront.targetVel.size >= 0) {
                fbcf_set_pos_spd(0x01, WheelType.LeftFront.targetVel.dir * 0.875944f,
                                 SLOWSTARTSPEED * 0.001f);
            }else {
                fbcf_set_pos_spd(0x01, WheelType.LeftFront.targetVel.dir * 0.875944f,
                                 -SLOWSTARTSPEED * 0.001f);
            }
            osDelay(3);
            // HAL_Delay(1);
            if(WheelType.LeftRear.targetVel.size >= 0) {
                fbcf_set_pos_spd(0x03, WheelType.LeftRear.targetVel.dir * 0.875944f,
                                 SLOWSTARTSPEED * 0.001f);
            }else {
                fbcf_set_pos_spd(0x03, WheelType.LeftRear.targetVel.dir * 0.875944f,
                                 -SLOWSTARTSPEED * 0.001f);
            }
            // fbcf_set_pos_spd(0x3,WheelType.LeftRear.targetVel.dir *0.875944f,-5.f);
            osDelay(3);
            // HAL_Delay(1);
            if(WheelType.RightRear.targetVel.size >= 0) {
                fbcf_set_pos_spd(0x02, WheelType.RightRear.targetVel.dir * 0.875944f,
                                 SLOWSTARTSPEED * 0.001f);
            }else {
                fbcf_set_pos_spd(0x02, WheelType.RightRear.targetVel.dir * 0.875944f,
                                 -SLOWSTARTSPEED * 0.001f);
            }
                // fbcf_set_pos_spd(0x2,WheelType.RightRear.targetVel.dir*0.875944f,-5);
                osDelay(3);
        }
        // HAL_Delay(1);

        // printf("%f\r\n",WheelType.RightRear.targetVel.size * 10);
#if LUN_MOTOR_LOCK_OFF == 0
    } else {
        // //自旋
        // WheelType.LeftFront.targetVel.dir = 90.f;
        // WheelType.LeftRear.targetVel.dir = -139.f;
        // WheelType.RightRear.targetVel.dir = -41.f;
        // printf("%f\t%f\t%f\t", (WheelType.LeftFront.targetVel.dir), (WheelType.LeftRear.targetVel.dir),
        // (WheelType.RightRear.targetVel.dir));
        // printf("%f\r%f\t%f\r\n",SteeringControl.Front.pos,SteeringControl.Left.pos,SteeringControl.Right.pos);
        fbcf_set_pos_spd(0x01, WheelType.LeftFront.targetVel.dir * 0.875944f, 0);
        osDelay(3);
        fbcf_set_pos_spd(0x03, WheelType.LeftRear.targetVel.dir * 0.875944f, 0);
        osDelay(3);
        fbcf_set_pos_spd(0x02, WheelType.RightRear.targetVel.dir * 0.875944f, 0);
        osDelay(3);
    }
#endif

    Last_F_Vel = WheelType.LeftFront.targetVel.size;
    Last_L_Vel = WheelType.LeftRear.targetVel.size;
    Last_R_Vel = WheelType.RightRear.targetVel.size;
#endif // MODE == 0

#if MODE == 1
    Motor_Speed(WheelType.LeftFront.targetVel.size,
                WheelType.LeftRear.targetVel.size,
                WheelType.RightRear.targetVel.size,
                WheelType.RightFront.targetVel.size);
    TEST = &WheelType;

//    Motor_Speed(10.f,10.f,10.f,10.f);
#if Printf_MotorSpeed == 1
    printf("vel1:%f\tvel2:%f\tvel3:%f\tvel4:%f\n",WheelType.LeftFront.targetVel.size,WheelType.LeftRear.targetVel.size,WheelType.RightRear.targetVel.size,WheelType.RightFront.targetVel.size);
#endif

#endif // MODE == 1
}

/**
 * @brief 判断是否有轮子超速
 * @param
*/
static void speedLimit(WheelSys *WheelType) {
    WheelType->LeftFront.targetVel.size = Limit_MaxAndMin(&WheelType->LeftFront.targetVel.size, MAXSPEED, -MAXSPEED);
    WheelType->RightFront.targetVel.size = Limit_MaxAndMin(&WheelType->RightFront.targetVel.size, MAXSPEED, -MAXSPEED);
    WheelType->LeftRear.targetVel.size = Limit_MaxAndMin(&WheelType->LeftRear.targetVel.size, MAXSPEED, -MAXSPEED);
    WheelType->RightRear.targetVel.size = Limit_MaxAndMin(&WheelType->RightRear.targetVel.size, MAXSPEED, -MAXSPEED);
}

/*********舵轮*****************舵轮*******************舵轮*********************舵轮************************************************************************************************/
/**
 * @brief get_total_dir   //获得转过的总角度
 * @param motor_measure* p
 * @return  获得转过的总角度
*/
static float get_total_dir(motor_measure p) {
    if (p.dir > p.last_dir) {
        p.total_dir += p.dir - p.last_dir;
    } else {
        p.total_dir -= p.last_dir - p.dir;
    }


    return p.total_dir;
}

/**
 * @brief 轮子输出角度(角度制)
 * @param
*/
void Output_dir(WheelSys WheelBase) {
#if MODE == 0
#if PrintfTotalDir == 1
    printf("Total1:%f\n", Wheel_Totaldir.Wheel1_Measure.total_dir);
    printf("Total2:%f\n", Wheel_Totaldir.Wheel2_Measure.total_dir);
    printf("Total3:%f\n", Wheel_Totaldir.Wheel3_Measure.total_dir);
    printf("Total4:%f\n", Wheel_Totaldir.Wheel4_Measure.total_dir);
#endif
    Steering2Zero.AngleFrontInit = ANGLE_INIT_F;
    Steering2Zero.AngleLeftInit = ANGLE_INIT_L;
    Steering2Zero.AngleRightInit = ANGLE_INIT_R;

    SteeringBase.LeftFront.targetVel.dir += Steering2Zero.AngleFrontInit;
    SteeringBase.LeftRear.targetVel.dir += Steering2Zero.AngleLeftInit;
    SteeringBase.RightRear.targetVel.dir += Steering2Zero.AngleRightInit;


#endif
}


/**
  * @brief  calSumWheelVel 计算合速度
  * @note
  * @param  transX：  X方向的平移速度
  * @param  transY：  Y方向的平移速度
  * @param  linearVel：线速度
  * @retval void
  **/

static void calSumWheelVel(WheelParam *pwheelParam, float transX, float transY, float lineVel) {
    //分别计算X、Y方向的分速度
    float sumVelX = transX + lineVel * cosf(Angle2Radian(pwheelParam->PositiveDir));
    float sumVelY = transY + lineVel * sinf(Angle2Radian(pwheelParam->PositiveDir));

    //计算合速度大小
    pwheelParam->targetVel.size = sqrtf(sumVelX * sumVelX + sumVelY * sumVelY);
    //    printf("size:%f Dir:%f lineVel:%f\r\n",pwheelParam->targetVel.size,pwheelParam->targetVel.dir,lineVel);
    //零向量判定
    (pwheelParam->targetVel).dir = (pwheelParam->targetVel).size > FLOAT_ZERO
                                       ? Radian2Angle(atan2f(sumVelY, sumVelX))
                                       : (pwheelParam->currentPos/* + 90.f */); //(此处+90) 与车系轮系之间建立的坐标关系有关

    //将合速度转为轮系坐标下的速度
    //pwheelParam->targetVel.dir -= 90.f;

    //限制180范围
    pwheelParam->targetVel.dir = AngleLimit180(pwheelParam->targetVel.dir);
}

/*
  * @brief  calWheelVel 计算每个轮子的速度
  * @note
  * @param  wheelSysVel：需要赋值轮系速度结构体变量
  * @param  vel：		 机器人坐标系的速度
  * @param  omega：		 角速度
  * @retval void
  */
static void calWheelVel(vector *vel, float omega) {
    if (vel != NULL) {
        // 1.分解平移速度
        float translationSpeedX = vel->size * cosf(Angle2Radian(vel->dir));
        float translationSpeedY = vel->size * sinf(Angle2Radian(vel->dir));

        // 2.计算轮子的线速度(顺正逆负)
        float LFLineVel = omega * (SteeringBase.LeftFront.RotationRadius);
        //        float RFLineVel = omega * (SteeringBase.RightFront.RotationRadius);
        float LRLineVel = omega * (SteeringBase.LeftRear.RotationRadius);
        float RRLineVel = omega * (SteeringBase.RightRear.RotationRadius);
        SteeringBase.LeftFront.currentPos = vel->dir;
        //        SteeringBase.RightFront.currentPos = vel->dir;
        SteeringBase.RightRear.currentPos = vel->dir;
        SteeringBase.LeftRear.currentPos = vel->dir;
        // 3.分别计算每个轮子的合速度(每个轮子需要给平移速度和自旋线速度) 并限制转角范围为-180~180
        calSumWheelVel(&(SteeringBase.LeftFront), translationSpeedX, translationSpeedY, LFLineVel);
        //        calSumWheelVel(&(SteeringBase.RightFront), translationSpeedX, translationSpeedY,  RFLineVel);
        calSumWheelVel(&(SteeringBase.LeftRear), translationSpeedX, translationSpeedY, LRLineVel);
        calSumWheelVel(&(SteeringBase.RightRear), translationSpeedX, translationSpeedY, RRLineVel);
    }
}

/**
  * @brief  wheelTurnMindir 优化转向电机转角
  * @note
  * @param  targetVel：轮子的速度
  * @param  currentMotorPos：当前电机的位置
  * @param  wheelId：对应方向的轮子
  * @retval void
  */
static void wheelTurnMindir(vector *targetVel, float currentMotorPos) {
    if (targetVel != NULL) {
        // 计算与目标位置中心对称位置
        targetVel->dir = AngleLimit180(targetVel->dir);
        float oppositePos = AngleLimit180(targetVel->dir + 180.f);
        //        float oppositePos = targetVel->dir + 180.f;

        // 将目标位置转换为同一个周期
        targetVel->dir = Angle2SameCycle180(targetVel->dir, currentMotorPos);
        oppositePos = Angle2SameCycle180(oppositePos, currentMotorPos);

        // 根据轮子位置反馈，找到离轮子正方向最近的那个角，因为两条相交直线的夹角总有一个是小于等于九十度的角
        float deltadir = AngleLimit180(targetVel->dir - currentMotorPos);
        float deltaOpdir = AngleLimit180(oppositePos - currentMotorPos);
        float absDeltadir = fabsf(deltadir);
        float absDeltaOpdir = fabsf(deltaOpdir);

        // 核心逻辑，增量法
        // 若原目标位置本来就是离电机位置最近
        if (absDeltadir <= absDeltaOpdir) {
            targetVel->dir = currentMotorPos + deltadir;
        }
        // 若目标位置的中心对称位置离电机位置最近, 速度反向
        else {
            targetVel->dir = currentMotorPos + deltaOpdir;
            targetVel->size = -targetVel->size;
        }
        if (TaskFlag.Flag_ManualMode) {
            targetVel->dir = AngleLimit180(targetVel->dir);
        }
        //        printf("size:%f dir:%f Now:%f ",SteeringBase.LeftFront.targetVel.size,SteeringBase.LeftFront.targetVel.dir,M2006.Now_Angle);
        //        printf("deltadir:%f deltaOpdir:%f  currentMotorPos:%f  targetDir:%f size:%f\r\n", deltadir,deltaOpdir,currentMotorPos, targetVel->dir,targetVel->size);
    }
}

/**
  * @brief  wheelSysPosJuge 优化转向电机转角
  * @note
  * @param  wheelSysVel：轮系的速度
  * @retval void
  */
static void wheelsysPosJuge(void) {
    SteeringBase.LeftFront.currentPos = SteeringControl.Front.pos - ANGLE_INIT_F;
    //    SteeringBase.RightFront.currentPos = Wheel_Totaldir.Wheel2_Measure.total_dir;
    SteeringBase.LeftRear.currentPos = SteeringControl.Left.pos - ANGLE_INIT_L;
    SteeringBase.RightRear.currentPos = SteeringControl.Right.pos - ANGLE_INIT_R;


    wheelTurnMindir(&(SteeringBase.LeftFront.targetVel), SteeringBase.LeftFront.currentPos);
    //    wheelTurnMindir(&(SteeringBase.RightFront.targetVel), SteeringBase.RightFront.currentPos);
    wheelTurnMindir(&(SteeringBase.LeftRear.targetVel), SteeringBase.LeftRear.currentPos);
    wheelTurnMindir(&(SteeringBase.RightRear.targetVel), SteeringBase.RightRear.currentPos);
}

/******************************************************************************************************************************************************************/
/**************************全向轮**************全向轮**************全向轮****************全向轮**********************            printf("wadwddqwfdqwfqwfqw\r\n");
**********************************************************************/

static void WheelSysMotorOutput(WheelSys *WheelType) {
#if MODE == 0
    //    Output_dir(*WheelType);//TODO

#endif // MODE == 0
    speedLimit(WheelType);
    Output_Speed(*WheelType);
}


#if MODE == 1
/**
 * @brief 全向轮解算
 * @param targetVel
 * @param omega
*/
static void calOminiVel(vector targetVel, float omega,WheelSys type)
{
    static float TranslationX = 0;
    static float TranslationY = 0;


    TranslationX = targetVel.size * cosf(Angle2Radian(targetVel.dir));

    TranslationY = targetVel.size * sinf(Angle2Radian(targetVel.dir));
//绝对
    type.LeftFront.targetVel.size = TranslationX * cosf(Angle2Radian( type.LeftFront.PositiveDir)    +  Angle2Radian( Robot_Pose.yaw) ) + TranslationY *  sinf(Angle2Radian(  type.LeftFront.PositiveDir)   +  Angle2Radian( Robot_Pose.yaw))   + omega ;
    type.LeftRear.targetVel.size = TranslationX * cosf(Angle2Radian(  type.LeftRear.PositiveDir)     +  Angle2Radian( Robot_Pose.yaw) ) + TranslationY *   sinf(Angle2Radian(type.LeftRear.PositiveDir)     +  Angle2Radian( Robot_Pose.yaw))   + omega ;
    type.RightRear.targetVel.size = TranslationX * cosf(Angle2Radian( type.RightRear.PositiveDir)    +  Angle2Radian( Robot_Pose.yaw) ) + TranslationY *  sinf(Angle2Radian(  type.RightRear.PositiveDir)   +  Angle2Radian( Robot_Pose.yaw))   + omega ;
    type.RightFront.targetVel.size = TranslationX * cosf(Angle2Radian(type.RightFront.PositiveDir)   +  Angle2Radian( Robot_Pose.yaw) ) + TranslationY * sinf(Angle2Radian(    type.RightFront.PositiveDir) +  Angle2Radian( Robot_Pose.yaw))   + omega ;
//
//相对
//    type.LeftFront.targetVel.size = TranslationX * cosf(Angle2Radian( type.LeftFront.PositiveDir)     ) + TranslationY * sinf(Angle2Radian(type.LeftFront.PositiveDir ) )   + omega * 40.f;
//    type.LeftRear.targetVel.size = TranslationX * cosf(Angle2Radian(  type.LeftRear.PositiveDir)      ) + TranslationY * sinf(Angle2Radian(type.LeftRear.PositiveDir  ) )  + omega * 40.f;
//    type.RightRear.targetVel.size = TranslationX * cosf(Angle2Radian( type.RightRear.PositiveDir)     ) + TranslationY * sinf(Angle2Radian(type.RightRear.PositiveDir ) )  + omega * 40.f;
//    type.RightFront.targetVel.size = TranslationX * cosf(Angle2Radian(type.RightFront.PositiveDir)    ) + TranslationY * sinf(Angle2Radian(type.RightFront.PositiveDir) )  + omega * 40.f;
    WheelSysMotorOutput(type);
}
#endif


/**
  * @brief  wheelSysOutput 运动解算入口函数
  * @note
  * @param  vel：		车体坐标的速度
  * @param	omega： 角速度
  * @retval void
  */
void wheelSysOutPut(vector vel, float omega) {
    //1.世界坐标角度转成机器人坐标系角度
#if MODE == 0
    vel.dir -= Robot_Pose.yaw;
#endif // MODE == 0

    //    printf("yaw:%f\n", getCurrentRobPose().yaw);
#if MODE == 0   //舵轮

    //2.在机器人坐标系下,计算每个轮子的速度,将四个轮子看为四个矢量
    calWheelVel(&vel, omega);

    //3.优化转向电机转角
    //    if((!flagLockL) && (!flagLockR) && (!flagLockF))
    wheelsysPosJuge();
    //4.输出到轮系电机上
    WheelSysMotorOutput(&SteeringBase);

#endif
#if MODE == 1   //全向轮
    calOminiVel(vel,omega,OminiBase);

//    WheelSysMotorOutput(OminiBase);

#endif
}

//自动泊车函数
//vector AutoParkTask() {
//    static vector OutVector = {0};
//    static int32_t X, Y = 0;
//    vector UX, UY = {0};
//    UX.dir = 0;
//    UY.dir = 90;
//    X = ultrasonicOne.distance;
//    Y = ultrasonicSecond.distance;
//    PID_Info ParkPIDX = {0};
//    PID_Info ParkPIDY = {0};
//    ParkPIDX.Kp = 200.F;
//    ParkPIDY.Kp = 200.F;
//    ParkPIDX.Target = 3;
//    ParkPIDY.Target = 24;
//    ParkPIDX.Now = (float) X;
//    ParkPIDY.Now = (float) Y;
//    PID_Calc(&ParkPIDX);
//    PID_Calc(&ParkPIDY);
//    UX.size = ParkPIDX.PID_OUT[0];
//    UX.size = Limit_MaxAndMin(&UX.size, 5000, -5000);
//    UY.size = ParkPIDY.PID_OUT[0];
//    UY.size = Limit_MaxAndMin(&UY.size, 5000, -5000);
//
//    OutVector = vectorSynthesis(&UX, &UY);
//    //    printf("UX:%d UY:%d ON:%d OUTX:%f OUTY:%f Dir:%f Size:%f\r\n",ultrasonicOne.distance,ultrasonicSecond.distance, HAL_GPIO_ReadPin(AUTO_PARK_IN_GPIO_Port,AUTO_PARK_IN_Pin),UX.size,UY.size,OutVector.dir,OutVector.size);
//    return OutVector;
//}
extern float key_yaw;
//机器人底盘运动总输出
void robotMoveTask() {
    //平移速度

    static bool once = false;

    static vector goPathVel = {0};
    static ChassisMovement adjustVector = {0};
    static vector endVel = {0};
    static float omega = 0.f;
    if (TaskFlag.Flag_ManualMode) {
        if (!once) {
            Manual_pidInit();
            once = true;
            // printf("use manual\r\n");
        }
        goPathVel = hand_shake.HandShake_Vec;
        setPidCalMode(PID_YAW_ONLY);
        static Pose Handle_Pose;
        Handle_Pose.yaw = hand_shake.Handshake_Yaw.yaw + key_yaw;
        // printf("%f\t%f\t%f\t%f\r\n",hand_shake.HandShake_Vec.dir,hand_shake.HandShake_Vec.size,hand_shake.Handshake_Yaw.yaw,key_yaw);
        setPidTargetPose(&Handle_Pose, PID_YAW_ONLY);
    } else if (TaskFlag.Flag_UseManualYaw) {
        if (!once) {
            Manual_pidInit();
            once = true;
            // printf("use manual\r\n");
        }
        setPidCalMode(PID_YAW_ONLY);
        goPathVel.dir = 0;
        goPathVel.size = 0;
        static Pose Handle_Pose;
        Handle_Pose.yaw = hand_shake.Handshake_Yaw.yaw;
        // printf("%f\r\n",hand_shake.Handshake_Yaw.yaw);
        setPidTargetPose(&Handle_Pose, PID_YAW_ONLY);
    } else {
        goPathVel = getPathVel();
    }
    if (once) {
        pidInit();
        once = false;
        // printf("use nomal\r\n");
    }
    adjustVector = getPidOutput();
    endVel = vectorSynthesis(&goPathVel, &adjustVector.transVel);
    // endVel.dir=0.f;
    // endVel.size = 0.f;
    omega = adjustVector.omega;
    // omega = 3000;
    wheelSysOutPut(endVel, omega);
    // printf("%f\r\n",omega);
#if Printf_TargetVel == 1
    printf("Size:%f\tDir:%f\tOmega:%f\r\n",endVel.size,endVel.dir,omega);
    // printf("    PathSize:%.2f PathVel:%.2f pid.size:%.2f pid.vel:%.2f omega:%.2f X:%.2f Y:%.2f Yaw:%.2f spd:%.2f pos:%.2f tpos:%.2f\r\n\r\n",goPathVel.size,goPathVel.dir,adjustVector.transVel.size,adjustVector.transVel.dir, omega,Robot_Pose.wx,Robot_Pose.wy,Robot_Pose.yaw,SteeringControl.Front.spd,SteeringControl.Front.pos,SteeringBase.LeftFront.targetVel.dir);
    // printf("%f\t%f\t%f\r\n",SteeringControl.Front.pos,SteeringControl.Left.pos,SteeringControl.Right.pos);
#endif
}
